﻿/**
* @file ConfigRW.c in FusionUnitXIH6
* @author rebeater
* @comment
* Create on 11/3/21 10:48 PM
* @version 1.0
**/

#include <string.h>
#include "confgrw.h"
#include <fatfs.h>
#include <nav_struct.h>
#include <stdlib.h>
#include <rtcommon.h>
static const int kMaxLineOfConfig = 255;
/**
 * read line of ascii file
 * @param fp
 * @param buff
 * @return bytes of data
 */
int Readline(FIL *fp, char *buff) {
  int i;
  UINT len;
  FRESULT res;
  for (i = 0; i < kMaxLineOfConfig; i++) {
    res = f_read(fp,buff+i,1,&len);
    if(res!= FR_OK) break;
	if (len <=0) break;
	if (buff[i] == '\n') {
	  buff[i + 1] = 0;
	  i++;
	  break;
	}
  }
  return i;
}
/**
 * convert unit of Option
 * @param opt
 */
void ConvertUnit(Option *opt) {
  opt->imuPara.arw = (float)(opt->imuPara.arw * _deg / _sqrt_h);
  opt->imuPara.vrw = (float)(opt->imuPara.arw / _sqrt_h);
  for (int i = 0; i < 3; i++) {
	opt->imuPara.ab_ini[i] = (float)(opt->imuPara.ab_ini[i] * _mGal);
	opt->imuPara.gb_ini[i] = (float)(opt->imuPara.gb_ini[i] * _deg / _hour);
	opt->imuPara.as_ini[i] = (float)(opt->imuPara.as_ini[i] * _ppm);
	opt->imuPara.gs_ini[i] = (float)(opt->imuPara.gs_ini[i] * _ppm);
  }
  opt->imuPara.ab_std = (float)(opt->imuPara.ab_std * _mGal);
  opt->imuPara.gb_std = (float)(opt->imuPara.gb_std * _deg / _hour);
  opt->imuPara.as_std = (float)(opt->imuPara.as_std * _ppm);
  opt->imuPara.gs_std = (float)(opt->imuPara.gs_std * _ppm);

  opt->imuPara.at_corr = opt->imuPara.at_corr * 3600;
  opt->imuPara.gt_corr = opt->imuPara.gt_corr * 3600;

  /*float -> int TODO 可能有坑，字符串 1 -> float:0.99999999 int: (int)0.999999=1*/
  opt->imu_data_rate = (int)*(float *)&(opt->imu_data_rate);
  opt->nhc_enable = (int)*(float *)&(opt->nhc_enable);
  opt->zupt_enable = (int)*(float *)&(opt->zupt_enable);
  opt->odo_enable = (int)*(float *)&(opt->odo_enable);
  opt->align_mode = (int)*(float *)&(opt->align_mode);
  opt->enable_gnss = (int)*(float *)&(opt->enable_gnss);
}
FIL ConfigFile ALIGN_RAM_D1;
/**
 *
 * @param path
 * @param opt
 * @return 0 OK  -1: open failed 1:open successfully but load failed,eg.: wrong number
 */
int LoadConfig(char *path, Option *opt) {

  FRESULT res = f_open(&ConfigFile, path, FA_READ);
  if (res!= FR_OK) return -1;
  char buff[256];
  char *end;
  char *p;
  float *popt = (float *)opt;
  while (Readline(&ConfigFile, buff) > 0 && (char *)popt < (char *)opt + sizeof(Option)) {
	if (buff[0] == '#') continue;
	if (buff[0] == '\n') continue;
	for (p = buff, end = p + 1, *popt = strtof(p, &end); p != end;) {
	  p = end;
	  popt++;
	  *popt = strtof(p, &end);
	}
  }
  f_close(&ConfigFile);
  ConvertUnit(opt);
  return !((strcmp(buff, END_OF_CONFIG) == 0) && (char *)popt == ((char *)opt + sizeof(Option)));
}
void CreateConfigFile(char *path,Option *opt){
  /*TODO*/
}